#include "pid.h"

// 清除控制器变量
void PIDController_Init(PIDController *pid) {

	pid->integrator = 0.0f;
	pid->prevError  = 0.0f;

	pid->differentiator  = 0.0f;
	pid->prevMeasurement = 0.0f;

	pid->out = 0.0f;
}

/**
 * @brief 
 * 
 * @param pid 
 * @param setpoint 目标值
 * @param measurement 测量值
 * @return float 
 */
float PIDController_Update(PIDController *pid, float setpoint, float measurement) {

	/*
	* 误差信号
	*/
    float error = setpoint - measurement;

	/*
	* 比例
	*/
    float proportional = pid->Kp * error;

	/*
	* 积分
	*/
    pid->integrator = pid->integrator + 0.5f * pid->Ki * pid->T * (error + pid->prevError);

	/* 积分器限位防缠绕 */
    if (pid->integrator > pid->limMaxInt) {

        pid->integrator = pid->limMaxInt;

    } else if (pid->integrator < pid->limMinInt) {

        pid->integrator = pid->limMinInt;

    }

	/*
	* 导数（带通微分器）
	*/
		
    pid->differentiator = -(2.0f * pid->Kd * (measurement - pid->prevMeasurement)	/* 注意：测量值导数，因此等式前面有减号！ */
                        + (2.0f * pid->tau - pid->T) * pid->differentiator)         /* 加入微分低通滤波器时常数tau，减少噪声影响 */
                        / (2.0f * pid->tau + pid->T);


	/*
	* 计算输出并应用限制
	*/
    pid->out = proportional + pid->integrator + pid->differentiator;

    if (pid->out > pid->limMax) {

        pid->out = pid->limMax;

    } else if (pid->out < pid->limMin) {

        pid->out = pid->limMin;

    }

	/* 存储误差和测量值以备后用 */
    pid->prevError       = error;
    pid->prevMeasurement = measurement;

	/* 返回控制器输出 */
    return pid->out;

}
